import rclpy
from rclpy.node import Node
import termios
import sys
import tty
import threading
from unitree_api.msg import Request
import json



moveBindings = {
    'w': (1, 0, 0, 0),#x*1 y*0 z*0 角速度*0
    'e': (1, 0, 0, -1),#右前转弯
    'a': (0, 0, 0, 1),
    'd': (0, 0, 0, -1),
    'q': (1, 0, 0, 1),
    's': (-1, 0, 0, 0),
    'c': (-1, 0, 0, 1),
    'z': (-1, 0, 0, -1),
    'E': (1, -1, 0, 0),
    'W': (1, 0, 0, 0),
    'A': (0, 1, 0, 0),
    'D': (0, -1, 0, 0),
    'Q': (1, 1, 0, 0),
    'S': (-1, 0, 0, 0),
    'C': (-1, -1, 0, 0),
    'Z': (-1, 1, 0, 0),
}
class TelepoNode(Node):
    def __init__(self):
        super().__init__('agiros_teleop_ctrl')
        self.pub = self.create_publisher(Request,"/api/sport/request",10)
        #设置线速度角速度初始值
        self.declare_parameter("speed",0.25) #x和y上线速度一致
        self.declare_parameter("angular",0.7)

        self.speed = self.get_parameter("speed").value
        self.angular = self.get_parameter("angular").value

    #发布速度指令
    def publish(self,api_id,x=0.0,y=0.0,z=0.0):
        req = Request()
        req.header.identity.api_id = api_id
        js = {"x": x,"y": y,"z": z}
        req.parameter = json.dumps(js)

        self.pub.publish(req)


#按键读取函数
def getKey(settings):
    #按键读取模式
    tty.setraw(sys.stdin.fileno())
    #获取一个按键
    key = sys.stdin.read(1)
    #恢复原始设置
    termios.tcsetattr(sys.stdin,termios.TCSADRAIN,settings)
    #返回按键值
    return key

def main():
    settings = termios.tcgetattr(sys.stdin)

    rclpy.init()
    teleopNode = TelepoNode()
    spinner = threading.Thread(target=rclpy.spin,args=(teleopNode,))
    spinner.start()

    #循环读取按键并映射
    #finally进行异常处理保障资源释放
    try:
        while True:
            key =getKey(settings)
            print(key)
            #站立
            if key == 'g':
                teleopNode.publish(1004)
            #打招呼
            elif key == 'h':
                teleopNode.publish(1016)
            #伸展
            elif key == 'j':
                teleopNode.publish(1017)
            #坐下
            elif key == 'k':
                teleopNode.publish(1009)
            #坐下恢复到站立
            elif key == 'l':
                teleopNode.publish(1010)


            #结束终端判断ctrl+c,卧姿
            elif key == '\x03':
                teleopNode.publish(1005)
                break

            #运动控制
            elif key in moveBindings.keys():
                x_bind = moveBindings[key][0]
                y_bind = moveBindings[key][1]
                z_bind = moveBindings[key][3]
                teleopNode.publish(1008,
                                  x= x_bind * teleopNode.speed,
                                  y= y_bind * teleopNode.speed,
                                  z= z_bind * teleopNode.angular
                                  )

    finally:
        rclpy.shutdown()
        #进入卧姿
        teleopNode.publish(1005)





if __name__ == '__main__':
    main()

